﻿#include "plotmanage.h"

PlotManage::PlotManage(QCustomPlot *plot):
    plot_points(0),
    line_colors{
        /* For channel data (gruvbox palette) */
        /* Light */
        QColor (0xfb4934),
        QColor (0xb8bb26),
        QColor (0xfabd2f),
        QColor (0x83a598),
        QColor (0xd3869b),
        QColor (0x8ec07c),
        QColor (0xfe8019),
        /* Light */
        QColor (0xcc241d),
        QColor (0x98971a),
        QColor (0xd79921),
        QColor (0x458588),
        QColor (0xb16286),
        QColor (0x689d6a),
        QColor (0xd65d0e),
     },
    channels(0)
{
    this->plot = plot;
    b_start = true;
    idMaps.insert("PhaseA Current", PHASEA_CURR_IDX);
    idMaps.insert("PhaseB Current", PHASEB_CURR_IDX);
    idMaps.insert("PhaseC Current", PHASEC_CURR_IDX);
    idMaps.insert("PhaseA Voltage", PHASEA_VOL_IDX);
    idMaps.insert("PhaseB Voltage", PHASEB_VOL_IDX);
    idMaps.insert("PhaseC Voltage", PHASEC_VOL_IDX);
    idMaps.insert("Input Torque", INPUT_TORQUE_IDX);
    idMaps.insert("Output Torque", OUTPUT_TORQUE_IDX);
    idMaps.insert("Input Velocity", INPUT_VELOCITY_IDX);
    idMaps.insert("Encoder Velocity", ENCODER_VELOCITY_IDX);
    idMaps.insert("Encoder Angle", ENCODER_ANGLE_IDX);
    idMaps.insert("Sensorless Angle", SENSORLESS_ANGLE_IDX);
    idMaps.insert("Sensorless Velocity", SENSORLESS_VELOCITY_IDX);
    idMaps.insert("D real Current", D_AXIS_CURR_IDX);
    idMaps.insert("Q real Current", Q_AXIS_CURR_IDX);
    idMaps.insert("D ref Current", D_TARGET_CURR_IDX);
    idMaps.insert("Q ref Current", Q_TARGET_CURR_IDX);
    idMaps.insert("Daxis Voltage", D_AXIS_VOL_IDX);
    idMaps.insert("Qaxis Voltage", Q_AXIS_VOL_IDX);
    idMaps.insert("PWM dutyA", SVPWM_DUTYA_IDX);
    idMaps.insert("PWM dutyB", SVPWM_DUTYB_IDX);
    idMaps.insert("PWM dutyC", SVPWM_DUTYC_IDX);
    idMaps.insert("DC Current", DC_CURRENT_IDX);
    idMaps.insert("Angle Diff", AGNLE_DIFF_IDX);
}

int PlotManage::AddGraph(QString name, QColor color, int dataIdx){
    /* Add new channel data */
    QCPGraph *n = plot->addGraph();
    graphs.insert(dataIdx, n);
    QPen pen;
    pen.setStyle(Qt::SolidLine);
    pen.setWidth(1);
    pen.setColor(color);
    n->setPen (pen);
    n->setName (name);
    ids.append(dataIdx);
    channels++;
    return channels;
}

int PlotManage::AddGraph(int dataIdx){
    QString name = idMaps.key(dataIdx);
    QColor color = line_colors[dataIdx%CUSTOM_LINE_COLORS];
    return AddGraph(name, color, dataIdx);
}

void PlotManage::removeGraph(int dataIdx){
    for(QMap<int, QCPGraph *>::Iterator iter = graphs.begin(); iter != graphs.end(); ++iter){
        if (iter.key() == dataIdx) {
            plot->removeGraph(graphs.value(dataIdx));
            graphs.remove(dataIdx);
            ids.removeOne(dataIdx);
            channels--;
            break;
        }
    }
    if (ids.size() == 0) {
        plot_points = 0;
    }
}

QList<int> &PlotManage::getIds(){
    return ids;
}


void PlotManage::replot() {
    b_reflash = true;
}

void PlotManage::DisplayData(u32 id_mask, QList<float> data, int value){
    if (!b_start) {
        return;
    }
    int index = 0;
    for (int i = 0; i < 32 && index < data.size(); i++) {
        if (id_mask & (1<<i)) {
            QCPGraph *n = graphs.value(i);
            if (n) {
                n->addData(plot_points, data.at(index));
                index++;
            }
        }
    }
    if (index > 0) {
        // make key axis range scroll with the data (at a constant range size of 8):
        //plot->xAxis->setRange(plot_points, 8, Qt::AlignRight);
        if ((plot_points % 30 == 0) || b_reflash) {
            plot->xAxis->setRange (plot_points - value, plot_points);
            plot->replot(QCustomPlot::rpQueuedRefresh);
            b_reflash = false;
        }
        plot_points++;
    }
}
